#include "mpc.h"
#include <iostream>
#include <chrono>
#include "matplotlibcpp.hpp"
#include "continuous_linear_system.h"
namespace plt = matplotlibcpp;
using namespace LinearMpc;
class Order1stSys : public ContinuousLinearSys
{
public:
	Order1stSys() = delete;
	Order1stSys(double time_const) : ContinuousLinearSys(2, 1, 0, 0), time_const_(time_const)
	{
		state_matrix_ << -1 / time_const_, 0,
			1, 0;
		control_matrix_ << 1 / time_const_,
			0;
	}

private:
	double time_const_;
};
int main()
{
	double rate = 10;
	Order1stSys test_sys(0.5);
	auto dis_sys = test_sys.discretization(1 / rate, "tustin");
	/*	Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
	Mpc test_mpc(dis_sys, 200, 1, 0, 0, 0);
	test_mpc.set_xu_affine({}, {}, {0}, {1.0}, 0, -50, 50);
	/*bool Mpc::update_mpc(
	const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
	const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, const Eigen::MatrixXd &x_ref,
	const Eigen::MatrixXd &distur_ref)*/
	Eigen::MatrixXd q(2, 2);
	q << 0.0, 0.0,
		0.0, 1.0;
	Eigen::MatrixXd r(1, 1);
	r << 0.0;
	Eigen::VectorXd x0(2);
	x0 << 0.0, 0.0;
	Eigen::VectorXd u_fb(1);
	u_fb << 0.0;
	Eigen::MatrixXd x_ref(2, 1);
	x_ref << 0.0, 1.0;
	Eigen::MatrixXd distur_ref(2, 0);
	test_mpc.update_mpc(q, r, x0, u_fb, x_ref, distur_ref);
	int max_iter = 10;
	for (int i = 0; i < max_iter; i++)
	{
		test_mpc.mpc_solve();
	}
	plt::plot(test_mpc.get_control_vector(0));
	plt::show();
	plt::plot(test_mpc.get_state_vector(0));
	plt::plot(test_mpc.get_state_vector(1));
	plt::show();
}